#include <sys/types.h>
#include <fcntl.h>
#include <arpa/inet.h>
#include <nlohmann/json.hpp>
#include <radar.hpp>

const char *radar::RADAR_DEVICE = "/dev/ttyTHS0";

constexpr int RADAR_STATE_DIM = 2;    // [distance, angle]
constexpr int RADAR_MEASUREMENT_DIM = 2; // [measured_distance, measured_angle]


void IO_Handler(int sig, siginfo_t *info, void *context) {
    if (SIGRTMIN != sig) {
        return;
    }

    if (POLL_IN == info->si_code) {
        {
            std::unique_lock<std::mutex> lock(radar::GetInstance()->mutex_);
            radar::GetInstance()->cv_.notify_all();
        }
    }
}

radar::radar():
    fd(-1),
    radar_data_len(0),
    detected_(false),
    accumulated_(false),
    radar_frame_(),
    ops_(),
    KalmanFilterBase(RADAR_STATE_DIM, RADAR_MEASUREMENT_DIM){
}

radar::~radar() {
    std::cout << "radar end" << std::endl;
    close(fd);
}

void radar::Init() {
    int flag = 0;

    fd = open(RADAR_DEVICE, O_RDWR | O_NOCTTY);
    if (fd == -1) {
        int retry_cnt;
        std::cerr << "Failed to open radar device" << std::endl;
        do {
            std::cerr << "Retrying to open radar device..." << std::endl;
            sleep(1);
            fd = open(RADAR_DEVICE, O_RDWR | O_NOCTTY);
            if (fd != -1) {
                break;
            }
            retry_cnt ++;
            if (retry_cnt > 10) break;
        }while(1);
        // goto err;
    }
    
    tcgetattr(fd, &ops_);
    
    cfmakeraw(&ops_);

    ops_.c_cflag &= ~CSIZE;
    // 2. 设置 CS8 (8 数据位)
    ops_.c_cflag |= CS8;

    // 3. 禁用奇偶校验 (cfmakeraw 应该已经做了，但再次确认无害)
    ops_.c_cflag &= ~PARENB;
    ops_.c_cflag &= ~CSTOPB; // 确保是1停止位 (cfmakeraw 通常也会设置)

    // 4. 设置 CLOCAL 和 CREAD (cfmakeraw 通常也会设置)
    ops_.c_cflag |= (CLOCAL | CREAD);

    // 5. 设置波特率 (使用 cfsetispeed 和 cfsetospeed 更标准)
    if (cfsetispeed(&ops_, B115200) < 0 || cfsetospeed(&ops_, B115200) < 0) {
        std::cerr << "Failed to set baud rate" << std::endl;
        goto err;
    }

    // --- 对于输入标志 c_iflag ---
    // cfmakeraw 通常会清除 ISTRIP, INPCK, IXON, IXOFF, IXANY 等
    // 但可以再次确认，特别是 ISTRIP
    ops_.c_iflag &= ~ISTRIP;  // 明确禁用剥离第 8 位
    ops_.c_iflag &= ~(IXON | IXOFF | IXANY); // 明确禁用软件流控
    ops_.c_iflag &= ~INPCK; // 禁用输入奇偶校验检查

    // --- 对于本地标志 c_lflag ---
    // cfmakeraw 会清除 ICANON, ECHO, ECHOE, ISIG 等

    // --- 对于控制字符 c_cc ---
    ops_.c_cc[VTIME] = 0;
    ops_.c_cc[VMIN] = 0;

    if(tcflush(fd, TCIOFLUSH) < 0) {
        std::cerr << "Failed to flush radar device" << std::endl;
        goto err;
    }

    if(tcsetattr(fd, TCSANOW, &ops_) < 0) {
        std::cerr << "Failed to set radar device attributes" << std::endl;
        goto err;
    }
    
    flag = fcntl(fd, F_GETFL);
    flag |= O_ASYNC;
    if(fcntl(fd, F_SETFL, flag) < 0) {
        std::cerr << "Failed to set radar device flags" << std::endl;
        goto err;
    }
    if(fcntl(fd, F_SETOWN, getpid()) < 0) {
        std::cerr << "Failed to set radar device owner" << std::endl;
        goto err;
    }
    if(fcntl(fd, F_SETSIG, SIGRTMIN) < 0) {
        std::cerr << "Failed to set radar device signal" << std::endl;
        goto err;
    }

    sig_.sa_sigaction = IO_Handler;
    sig_.sa_flags = SA_SIGINFO;
    sigemptyset(&sig_.sa_mask);
    sigaction(SIGRTMIN, &sig_, NULL);

    radar_data.reserve(128); // Reserve space for radar data

    std::cout << "Radar initialized successfully. fd = " << fd << std::endl;

    return;
err:
    if (fd != -1) {
        std::cerr << "Failed to open radar device" << std::endl;
        close(fd);
        fd = -1;
    }
    std::cerr << "Radar initialization failed." << std::endl;
}

void radar::KalmanInit() {
    Eigen::MatrixXf F(RADAR_STATE_DIM, RADAR_MEASUREMENT_DIM);
    F << 1, 0,
         0, 1;
    setStateTransitionMatrix(F);

    Eigen::MatrixXf H(RADAR_MEASUREMENT_DIM, RADAR_STATE_DIM);
    H << 1, 0,
         0, 1;
    setMeasurementMatrix(H);

    Eigen::VectorXf process_noise(RADAR_STATE_DIM);
    process_noise << 0.1f, 0.1f;
    setProcessNoise(process_noise);

    Eigen::VectorXf measurement_noise(RADAR_MEASUREMENT_DIM);
    measurement_noise << 60.0f, 40.0f;
    setMeasurementNoise(measurement_noise);

    Eigen::VectorXf maxchange(RADAR_STATE_DIM);
    maxchange << 100.0f, 30.0f;
    setMaxStateChanges(maxchange);

    Eigen::VectorXf initial_state(RADAR_STATE_DIM);
    initial_state << 20.0f, 10.0f;
    Eigen::MatrixXf initial_covariance(RADAR_STATE_DIM, RADAR_STATE_DIM);
}

void radar::OnReceiveData(std::function<void(std::vector<uint8_t>& buffer)> callback) {
    callback_ = callback;
}

void radar::buffer_to_radar_frame(std::vector<uint8_t>& buffer) {
    if (buffer.size() < sizeof(radar_frame_)) {
        // std::cerr << "Buffer size is too small" << std::endl;
        return;
    }

    struct radar_frame *frame = reinterpret_cast<struct radar_frame *>(buffer.data());

    radar_frame_.MessageHeader  = ntohl(frame->MessageHeader);
    radar_frame_.PacketLength   = ntohs(frame->PacketLength);
    radar_frame_.SequenceID     = ntohs(frame->SequenceID);
    radar_frame_.RequestCommand = ntohs(frame->RequestCommand);
    radar_frame_.VersionID      = ntohs(frame->VersionID);
    radar_frame_.AnchorID       = ntohl(frame->AnchorID);
    radar_frame_.TagID          = ntohl(frame->TagID);
    radar_frame_.Distance       = ntohl(frame->Distance);
    radar_frame_.Azimuth        = ntohs(frame->Azimuth);
    radar_frame_.Elevation      = ntohs(frame->Elevation);
    radar_frame_.TagStatus      = ntohs(frame->TagStatus);
    radar_frame_.BatchSn        = ntohs(frame->BatchSn);
    radar_frame_.XorByte        = buffer[sizeof(struct radar_frame) - 1];

    Eigen::VectorXf rtMeas(RADAR_MEASUREMENT_DIM);
    rtMeas << radar_frame_.Distance, radar_frame_.Azimuth;
    predict();
    update(rtMeas);

    radar_frame_.Distance = getDistance();
    radar_frame_.Azimuth = getAngle();
    
    PrintRadarFrame(radar_frame_);
}


void radar::Start() {
    if(fd < 0) {
        std::cerr << "Radar device not initialized" << std::endl;
        return;
    }

    void_task_futures_.clear();
    void_task_futures_.push_back(std::async(std::launch::async, &radar::radar_handle, this));
}

void radar::Stop() {
    if (fd >= 0) {
        close(fd);
        fd = -1;
    }
}

void radar::radar_handle() {
    while (true) {
        {
            std::unique_lock<std::mutex> lock(mutex_);
            cv_.wait(lock);
        }
        size_t buffer_capacity = this->radar_data.capacity(); 
        if(buffer_capacity == 0) {
            this->radar_data.reserve(128); // Reserve space for radar data
            std::cerr << "Buffer capacity is zero" << std::endl;
            continue;
        }

        /* 如果接收长度与帧长度段不匹配，则等待下一次读取或抛弃这一帧 */
        this->radar_data.resize(buffer_capacity);
        int bytes_read = read(this->fd, this->radar_data.data(), buffer_capacity);
        if (bytes_read > 0) {
            if(this->accumulated_) {
                if(this->radar_frame_.PacketLength == this->accumulated_data_.size() + bytes_read) {
                    this->accumulated_ = false;
                    this->radar_data.resize(this->radar_frame_.PacketLength);
                    this->radar_data.insert(this->radar_data.begin(), this->accumulated_data_.begin(), this->accumulated_data_.end());
                    this->radar_data_len = this->radar_frame_.PacketLength;
                }
                else {
                    this->accumulated_ = false;
                    this->accumulated_data_.clear();
                    this->accumulated_data_.resize(0);
                    this->radar_data.clear();
                }
            }
            else { 
                this->radar_frame_.PacketLength = this->radar_data[5] | (this->radar_data[4] << 8);
                if(bytes_read < this->radar_frame_.PacketLength) {
                    this->accumulated_ = true;
                    this->accumulated_data_.resize(bytes_read);
                    this->accumulated_data_.assign(this->radar_data.begin(), this->radar_data.begin() + bytes_read);
                    continue;
                }
                else if (bytes_read == this->radar_frame_.PacketLength) {
                    this->radar_data_len = bytes_read;
                }
            }
            
            this->radar_data.resize(this->radar_data_len);
            
            if (this->callback_) {
                this->callback_(this->radar_data);
            }
        }
    }
}

float radar::getDistance() const {
    if (isInitialized())
        return x_(0);
    return 0.0f;
}

float radar::getAngle() const {
    if (isInitialized()) 
        return x_(1);
    return 0.0f;
}

void radar::SetDetected(bool detected) {
    detected_ = detected;
}

const struct radar_frame *radar::GetRadarFrame() {
    return &radar_frame_;
}

void radar::PrintRadarFrame(const struct radar_frame &frame) {
    // std::cout << "MessageHeader: " << std::hex << frame.MessageHeader << std::endl;
    // std::cout << "PacketLength: " << std::dec << frame.PacketLength << std::endl;
    // std::cout << "SequenceID: " << frame.SequenceID << std::endl;
    // std::cout << "RequestCommand: " << frame.RequestCommand << std::endl;
    // std::cout << "VersionID: " << frame.VersionID << std::endl;
    // std::cout << "AnchorID: " << frame.AnchorID << std::endl;
    // std::cout << "TagID: " << frame.TagID << std::endl;
    std::cout << "Distance: " << frame.Distance << std::endl;
    std::cout << "Azimuth: " << frame.Azimuth << std::endl;
    // std::cout << "Elevation: " << frame.Elevation << std::endl;
    // std::cout << "TagStatus: " << frame.TagStatus << std::endl;
    // std::cout << "BatchSn: " << frame.BatchSn << std::endl;
}

bool radar::GetDetected() {
    return detected_;
}

void radar::setFollowDistance(float distance) {
    follow_distance_ = distance;
}
void radar::setFollowAngle(float angle) {
    follow_angle_ = angle;
}

float radar::getFollowDistance() const {
    return follow_distance_;
}

float radar::getFollowAngle() const {
    return follow_angle_;
}
